{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib\n",
    "matplotlib.use('nbagg')\n",
    "import matplotlib.animation as anm\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "import matplotlib.patches as patches\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "class World:\n",
    "    def __init__(self, debug=False):\n",
    "        self.objects = [] \n",
    "        self.debug = debug\n",
    "        \n",
    "    def append(self,obj):\n",
    "        self.objects.append(obj)\n",
    "    \n",
    "    def draw(self):\n",
    "        fig = plt.figure(figsize=(4,4))  \n",
    "        ax = fig.add_subplot(111)  \n",
    "        ax.set_aspect('equal') \n",
    "        ax.set_xlim(-5,5) \n",
    "        ax.set_ylim(-5,5) \n",
    "        ax.set_xlabel(\"X\",fontsize=10) \n",
    "        ax.set_ylabel(\"Y\",fontsize=10)  \n",
    "        \n",
    "        elems = []\n",
    "        time_increment = 0.1\n",
    "        \n",
    "        if self.debug:        \n",
    "            for i in range(1000): self.one_step(i, elems, ax) \n",
    "        else:\n",
    "            self.ani = anm.FuncAnimation(fig, self.one_step, fargs=(elems, ax), frames=100, interval=1000, repeat=False)\n",
    "            plt.show()\n",
    "        \n",
    "    def one_step(self, i, elems, ax):    ### fig:one_step1 (27-29行目) fig:one_step2 （27-31行目） \n",
    "        while elems: elems.pop().remove()\n",
    "        elems.append(ax.text(-4.4, 4.5, \"t = \"+str(i), fontsize=10)) #座標ベタ書きが気になるなら変数に\n",
    "        for obj in self.objects: # 追加\n",
    "            obj.draw(ax, elems)  # 追加"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IdealRobot:\n",
    "    def __init__(self, pose, color=\"black\"):\n",
    "        self.pose = pose\n",
    "        self.r = 0.2\n",
    "        self.color = color\n",
    "    \n",
    "    def draw(self, ax, elems):                ### fig:append_elements (7-13行目)\n",
    "        x, y, theta = self.pose\n",
    "        xn = x + self.r * math.cos(theta)\n",
    "        yn = y + self.r * math.sin(theta)\n",
    "        elems += ax.plot([x,xn], [y,yn], color=self.color) # elems += を追加\n",
    "        c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color) # c = を追加\n",
    "        elems.append(ax.add_patch(c))   # elem.appendで包む"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "world = World(debug=True)        \n",
    "\n",
    "robot1 = IdealRobot( np.array([2, 3, math.pi/6]).T )           # ロボットのインスタンス生成（色を省略）\n",
    "robot2 = IdealRobot( np.array([-2, -1, math.pi/5*6]).T, \"red\")  # ロボットのインスタンス生成（色を指定）\n",
    "world.append(robot1)                                      # ロボットを登録 \n",
    "world.append(robot2)\n",
    "world.draw()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
